
import math
import pygame
import numpy as np
from world import *
import random 

PDF_PARTICLE_COUNT=1000
PARTICLE_COUNT=4000
zhit=0.95
zshort=0.2
zmax=0.2
zrandom=0.1 



class Particle:

      def __init__(self,world,pose):
          self.world=world
          self.pose=pose
         
          self.sonarList=[Sonar(self.world,0,3,(255,0,0)),Sonar(self.world,math.pi/2),Sonar(self.world,math.pi),Sonar(self.world,math.pi*3/2)]
          #self.sonarList=[Sonar(self.world,0,3,(255,0,0))]

          self.r=0
      def update(self):
          
          for s in self.sonarList:
               s.update(self.pose) 
    
      def getP(self,sonarRealValueList):
                 
          p=1

          if len(self.sonarList)!=len(sonarRealValueList):
             print "problems"

          for i in range(len(self.sonarList)):
              s = self.sonarList[i]
              realValue=sonarRealValueList[i]
                   
              p*=s.getP(realValue)   
  

          return p
       
      def draw(self,surface):
          
          if self.r==0:
             c=(0,255,0)   
          else:
             c=(255,0,0) 
              
          #for s in self.sonarList:
          #    s.draw(surface)          
 
          pygame.draw.circle(surface,c,(int(self.pose[0]/self.world.resolution),int(self.pose[1]/self.world.resolution)),4)   
              
              
          


class Robot:
      def __init__(self,world,pose,size,color,wheelDistance,ratioWheel):
          self.world=world
          self.pose=pose
          self.posePDF=[]
          self.particleList=[]
          self.size=size
          self.color=color 
          self.wheelDistance=wheelDistance
          self.ratioWheel=ratioWheel
  
          self.surface=pygame.Surface((self.size[0]/self.world.resolution,self.size[1]/self.world.resolution),pygame.HWSURFACE | pygame.SRCALPHA)
          self.tempSurface=self.surface

          self.sonarList=[Sonar(self.world,0,3,(255,0,0)),Sonar(self.world,math.pi/2),Sonar(self.world,math.pi),Sonar(self.world,math.pi*3/2)]
          #self.sonarList=[Sonar(self.world,0,3,(255,0,0))]

          self.c=0 
          self.initParticleList()          

 
      def initParticleList(self):
          
          for i in range(PARTICLE_COUNT):
              x=np.random.uniform(0,10)
              y=np.random.uniform(0,10)
 
              p=np.random.uniform(0,2*np.pi)
              #x=self.pose[0]+0.2
              #y=self.pose[1]+0.2    
              p=self.pose[2] 
              self.particleList.append(Particle(self.world,(x,y,p)))      
      
      def update(self,encoderCountLeft,encoderCountRight):
          self.encoderCountLeft=encoderCountLeft
          self.encoderCountRight=encoderCountRight 

          pose=self.computePoseFromEncoders() 
      
          motionModel=self.computeMotionModel(pose)


          self.pose=self.motionModelToPose(self.pose,motionModel)
          

          for s in self.sonarList:
               s.update(self.pose) 
    
          sonarRealValueList=[]
          for s in self.sonarList:
              sonarRealValueList.append(s.actualRange)
 
          self.updateFilter(motionModel,sonarRealValueList)
 
          #print self.sonarList[0].getP(self.sonarList[0].actualRange)
 
     
      def updateFilter(self,motionModel,sonarRealValueList):
       
          t0=pygame.time.get_ticks() 
          probList=[]
 
          for p in self.particleList:
              t01=pygame.time.get_ticks()
              newPose=self.motionModelToPose(p.pose,motionModel)
              p.pose=newPose
              p.update()

              probList.append(p.getP(sonarRealValueList))
              t02=pygame.time.get_ticks()
              #print t02-t01


          total=sum(probList)

          weightList=[]  

          for i  in range(len(probList)):
              weightList.append(probList[i]/total)


          t1=pygame.time.get_ticks()
                    
          count=len(self.particleList)

          p3 = []
          index = np.random.randint(0,count)
          beta = 0.0
          mw = max(weightList)
          for i in range(count):
            beta += random.random() * 2.0 * mw
            while beta > weightList[index]:
                beta -= weightList[index]
                index = (index + 1) % count

            p=self.particleList[index]
            par=Particle(self.world,(p.pose[0],p.pose[1],p.pose[2]))
            par.r=0
            p3.append(par)


          self.particleList=p3    
          self.c+=1
 
          #if self.c>2:
          #   self.c=0
          #   for i in range(50):
          #     x=np.random.uniform(0,10)
          #     y=np.random.uniform(0,10)
 
          #     p=np.random.uniform(0,2*np.pi)
          #     par=Particle(self.world,(x,y,p))
          #     par.r=1
          #     self.particleList.append(par)        
          tt=pygame.time.get_ticks()
          print tt-t0 
   
           

      def computePoseFromEncoders(self):
       


          aS=self.ratioWheel*(self.encoderCountLeft + self.encoderCountRight)/2;
          aPhi=self.ratioWheel*(self.encoderCountLeft - self.encoderCountRight) / self.ratioWheel;
 
          #x=self.pose[0]
          #y=self.pose[1]
          #phi=self.pose[2]

          xTemp=math.cos(aPhi)*aS
          yTemp=math.sin(aPhi)*aS
          phiTemp=aPhi
            
          phiTemp=phiTemp % (np.pi*2)    
         
          #print yTemp  
 
          

          #del self.posePDF[:]
          
          #for i in range(PDF_PARTICLE_COUNT):
          #    self.posePDF.append(self.motionModelToPose(self.pose,motionModel))
         
  
          #self.pose=self.motionModelToPose(self.pose,motionModel)
          return (xTemp,yTemp,phiTemp) 

                    
 


      def motionModelToPose(self,pose,motionModel):
          
          rot1=motionModel[0]
          trans1=motionModel[1]
          rot2=motionModel[2]  

          sampleRot1=np.random.normal(scale=0.01)
          sampleTrans1=np.random.normal(scale=0.01)
          sampleRot2=np.random.normal(scale=0.01)


          rot1-=sampleRot1
          trans1-=sampleTrans1
          rot2-=sampleRot2            
 

          x=pose[0]+trans1*math.cos(pose[2]+rot1) 
          y=pose[1]-trans1*math.sin(pose[2]+rot1) 
          phi=pose[2]+rot1+rot2

          return (x,y,phi) 
            

      def computeMotionModel(self,pose):
          '''
          Compute MotionModel for a pose
          
          self --- self
          pose --- robot pose  
          ''' 
          rot1=0          
          trans1=0
          rot2=0 

          if pose[0]!=0 or pose[1]!=0:
             rot1=math.atan2(pose[1],pose[0])
          
          trans1=math.sqrt(pose[0]**2+pose[1]**2) 
         
          #print pose[2]
 
          rot2=(pose[2]-rot1) % (np.pi*2)

                   

          return (rot1,trans1,rot2) 
                 
           

     
 
      def hitWall():
          pass 

      def draw(self,surface):
          #pygame.draw.rect(self.surface,self.color,(0,0,self.size[0]/self.world.resolution,self.size[1]/self.world.resolution),0)
          #self.tempSurface=pygame.transform.rotate(self.surface,self.pose[2]*180/math.pi)
          #surface.blit(self.tempSurface,(int(self.pose[0]/self.world.resolution),int(self.pose[1]/self.world.resolution)))
          #pygame.draw.line(surface,(255,0,0),(int(self.pose[0]/self.world.resolution),int(self.pose[1]/self.world.resolution)),(10,10)) 
          

          pygame.draw.circle(surface,(255,0,0),(int(self.pose[0]/self.world.resolution),int(self.pose[1]/self.world.resolution)),10)

          #for p in self.posePDF:
          #    pygame.draw.circle(surface,(0,255,0),(int(p[0]/self.world.resolution),int(p[1]/self.world.resolution)),1)   
 

          for s in self.sonarList:
              s.draw(surface)          
 
          for p in self.particleList:
              p.draw(surface) 

          
 
          
class Move2D:
      def __init__(self,rot1,trans1,rot2):
          self.rot1=rot1
          self.trans1=trans1
          self.rot2=rot2
      def __str__(self):
          s=StringIO()
          print >> s ,"(rot1:",self.rot1," trans1:",self.trans1," rot2:",self.rot2,")"
          return s.getValue() 


class Sonar:
      def __init__(self,world,orientation,_range=3,color=(0,0,255)):
 
          self.world=world
          self.orientation=orientation
          self._range=_range
          self.color=color
          

          self.update((0,0,0))

 
      def update(self,pose):
          self.x=pose[0]
          self.y=pose[1]
          self.absoluteOrientation=(self.orientation+pose[2]) % (math.pi*2)

          
          
          self.xDrawStart=int(self.x/self.world.resolution)
          self.yDrawStart=int(self.y/self.world.resolution)

          self.xDrawEnd=self.xDrawStart+int(math.sin(self.absoluteOrientation)*self._range/self.world.resolution)
          self.yDrawEnd=self.yDrawStart+int(math.cos(self.absoluteOrientation)*self._range/self.world.resolution) 
            
 
          coords=self.bresenham_line((self.xDrawStart,self.yDrawStart),(self.xDrawEnd,self.yDrawEnd))
          
       
          for c in coords:
              if c[0]>=600 or c[1]>=600:
                 break   
               
              if self.world.mapData[c[0]][c[1]]==1:
                 self.xDrawEnd=c[0]
                 self.yDrawEnd=c[1]
                 
                 break
          
          self.actualRange=math.sqrt((self.xDrawStart-self.xDrawEnd)**2+(self.yDrawStart-self.yDrawEnd)**2)*self.world.resolution

      
      def getP(self,value):
          

          pHit=0
          pshort=0
          pMax=0
          pRandom=0

          # p hit

          delta =np.fabs(value-self.actualRange)
          sigmaHit=0.2
          pHit=math.exp(-(delta**2)/(sigmaHit**2))  


          #print pHit,"-"

          # p max
          
          #print value,self._range  

          if self._range-value<0.02:
             pMax=1
          else:
             pMax=0  
    

          # p random

          if self._range-value<0.02:
             0
          else:
             pRandom=1/self._range


          p=zhit*pHit+zshort*pshort+zmax*pMax+zrandom*pRandom

          return p
      

           
             
  
      def draw(self,surface):
          #pygame.draw.line(surface,(255,255,0),(self.x,self.y),(5,500),1)
          pygame.draw.line(surface,self.color,(self.xDrawStart,self.yDrawStart),(self.xDrawEnd,self.yDrawEnd),4)
          
      def bresenham_line(self,(x,y),(x2,y2)):
          """Brensenham line algorithm"""
          steep = 0
          coords = []
          dx = abs(x2 - x)
          if (x2 - x) > 0: 
             sx = 1
          else: 
             sx = -1
          dy = abs(y2 - y)
          if (y2 - y) > 0:
             sy = 1
          else: 
             sy = -1
         
          if dy > dx:
             steep = 1
             x,y = y,x
             dx,dy = dy,dx
             sx,sy = sy,sx
    
          d = (2 * dy) - dx
          for i in range(0,dx):
             if steep: 
                coords.append((y,x))
             else: 
                coords.append((x,y))
             
             while d >= 0:
                 y = y + sy
                 d = d - (2 * dx)
             
             x = x + sx
             d = d + (2 * dy)
         
          coords.append((x2,y2)) 
          return coords

 

               
